#include "model/trunk_model.h"

void TrunkModel::init() {
	emotion_handler_sub_ = nh_.subscribe(Topic::EMO_HANDLER_TRUNK, 1, &TrunkModel::handlerCallback, this);

	joint_state_.name.resize(5);
	joint_state_.position.resize(5);

	joint_state_.name[0] = Joint::TRUNKLINK_1;
	joint_state_.name[1] = Joint::TRUNKLINK_2;
	joint_state_.name[2] = Joint::TRUNKLINK_3;
	joint_state_.name[3] = Joint::TRUNKLINK_4;
	joint_state_.name[4] = Joint::TRUNKLINK_5;
}

void TrunkModel::spin() {
	ros::Rate loop_rate(30);
	setInPosition(true);

	while (ros::ok()) {
		if (!isInPosition()) {
			moveTrunk();
		}

		prepState();
		joint_pub_.publish(joint_state_);
		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Handler Callback
 * Receives which position the trunk has to be in.
 *
 * @param		state	Int32, 0 = NEUTRAL, 1 = UP, 2 = DOWN.
 */
void TrunkModel::handlerCallback(const comm::trunk::ConstPtr &state) {
	int pos = state->state;

	if (position_ != pos) {
		position_ = pos;
		setInPosition(false);
	} else if (pos == TRUNK_NEUTRAL) {
		position_ = pos;
		angle_ = state->angle;
	}
}

void TrunkModel::moveTrunk() {
	if (position_ == TRUNK_DOWN) {
		if (angle_ > ModelValues::TRUNK_MIN_ROTATION) {
			angle_ -= ModelValues::TRUNK_STEP;
		}
	} else {
		setInPosition(true);
	}
}

void TrunkModel::prepState() {
	joint_state_.header.stamp = ros::Time::now();
	joint_state_.position[0] = angle_;
	joint_state_.position[1] = angle_;
	joint_state_.position[2] = angle_;
	joint_state_.position[3] = angle_;
	joint_state_.position[4] = angle_;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "trunk_model_publisher");

	TrunkModel trunk;
	trunk.init();
	trunk.spin();
}
